#include "stm32f10x.h"                  // Device header
#include "PID.h"
#include "main.h"
PID mypid;

void PID_Init(PID *pid, float *PID_P)
{
    pid->kp = PID_P[0];
    pid->ki = PID_P[1];
    pid->kd = PID_P[2];
    pid->maxIntegral = PID_P[3];
    pid->maxOutput = PID_P[4];
}

float PID_Calc(PID *pid,float TARGET,float REALVAL)
{
 	//更新数据
    pid->lastError = pid->error;//将旧error存起来
    pid->error = TARGET - REALVAL;//计算新error
    //计算比例
    float Pout = pid->error * pid->kp;
    //计算积分
    float Iout = Iout + pid->error * pid->ki;
	//计算微分
    float Dout=(pid->error - pid->lastError) * pid->kd;
    //积分限幅
    if(Iout > pid->maxIntegral) 
		  Iout = pid->maxIntegral;
    else if(Iout < -pid->maxIntegral) 
		  Iout = -pid->maxIntegral;
    //计算输出
    pid->output = Pout + Dout + Iout;
    //输出限幅
    if(pid->output > pid->maxOutput) 
		  pid->output = pid->maxOutput;
    else if(pid->output < -pid->maxOutput) 
		  pid->output = -pid->maxOutput;
	
	return pid->output;
}
